%% Passive Force Profile

function [time, force, output] = passive_force_profile()

phases = 3;
output = cell(phases, 2);

x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, evalin('base','td_angle'), 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];

% controller = ComControllerClass;
controller = SimpleControllerClass;

sim = ForceControlModelClass(x0, @flat_rigid_environment, controller);

for i = 1 : phases
    [output{i, 1}, output{i, 2}] = sim.run();
end

x = output{2,2};

r = sqrt( ( x(:,sim.yci) - x(:,sim.yti) ).^2 + ...
    ( x(:,sim.xci) - x(:,sim.xti) ).^2 );

time = output{2,1}-output{2,1}(1);
force = - sim.kl .* r  ...
            + sim.kl .* sim.rl .* x(:,sim.oli) ./ sim.G;

% anim = AnimateForceControlModelClass(sim.gnd, output);
% anim.animate;

end